/*控制器*/
#ifndef CONTROLLER_H
#define CONTROLLER_H

#include <iostream>
#include "simucpp.hpp"
using namespace simucpp;
using namespace std;

/*PID控制器*/
class PID_Controller: public PackModule {
public:
    PID_Controller() {};
    PID_Controller(Simulator *sim, double Kp=1, double Ki=0, double Kd=0, double N=100)
        :_Kp(Kp), _Ki(Ki), _Kd(Kd), _N(N) {
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(uint32_t n) const override {
        if (n==0) return mdin1;
        return nullptr;
    };
    virtual PUnitModule Get_OutputPort(uint32_t n) const override {
        if (n==0) return mdout1;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        SUIntegrator(mdintegral, sim);
        SUGain(mdin1, sim);
        SUSum(mdout1, sim);
        SUSum(mdiffsum, sim);
        SUIntegrator(mdiffint, sim);
        sim->connectU(mdin1, mdintegral);
        sim->connectU(mdin1, mdiffsum);
        sim->connectU(mdiffsum, mdiffint);
        sim->connectU(mdiffint, mdiffsum);
        sim->connectU(mdin1, mdout1);
        sim->connectU(mdintegral, mdout1);
        sim->connectU(mdiffsum, mdout1);
        mdout1->Set_InputGain(_Kp, 0);
        mdout1->Set_InputGain(_Ki, 1);
        mdout1->Set_InputGain(_Kd, 2);
        mdiffsum->Set_InputGain(-_N, 1);
    }
    void Set_Param(vecdble param) {
        if (param.size()!=4) {
            cout<<"error"<<endl;
            return;
        }
        _Kp=param[0];_Ki=param[1];_Kd=param[2];_N=param[3];
        mdout1->Set_InputGain(_Kp, 0);
        mdout1->Set_InputGain(_Ki, 1);
        mdout1->Set_InputGain(_Kd, 2);
        mdiffsum->Set_InputGain(-_N, 1);
    }
    double _Kp=1, _Ki=0, _Kd=0, _N=100;
    UIntegrator *mdintegral=nullptr;
    UGain *mdin1=nullptr;
    USum *mdout1=nullptr;
    USum *mdiffsum=nullptr;
    UIntegrator *mdiffint=nullptr;
};


/*LQR控制器*//*
class LQR_Controller: public PackModule
{
public:
    LQR_Controller() {};
    LQR_Controller(Simulator *sim, double *K, int order){
        _order = (order>=2) ? order : 2;
        _K = new double[_order];
        for(int i=0;i<_order;i++) _K[i]=K[i];
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(int n) const {
        if (n==_order) return miou;
        if (n>_order) return nullptr;
        return mio[n];
    };
    virtual PUnitModule Get_OutputPort(int n) const {
        if (n==0) return msum1;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        mio = new MConnector*[_order];
        for(int i=0;i<_order;i++)
            mio[i] = new MConnector(sim, "mio"+to_string(i));
        SMConnector(miou, sim);
        SMSum(msum1, sim);
        for(int i=0;i<_order;i++)
            sim->connect(mio[i], msum1);
        sim->connect(miou, msum1);
        for(int i=0;i<_order;i++)
            msum1->Set_InputGain(_K[i], i);
        msum1->Set_InputGain(0, _order);
    }
    int _order;
    double *_K;
    MConnector **mio=nullptr;  // 输入状态
    MConnector *miou=nullptr;  // 输入指令
    MSum *msum1=nullptr;
};
*/
#endif
